function [ in ] = sensorout2in_newton_simp( DupsilonTildeOutSl, KScal0, dfBiasS, dKScalP, dPSS0, stepNum ) %#codegen
%SENSOROUT2IN_NEWTON_SIMP 将器件的输出值按牛顿迭代法转换为输入值
%
% Tips:
% 1. 对增量计算，传入的dfBiasS需要乘以采样周期，传入的dKScalP(:,2)需要除以采样周期
%
% Input Arguments:
% DupsilonTildeOutSl: 3*n矩阵，n为采样个数，器件输出值
% KScal0: 3*1向量，标度因数
% dfBiasS: 3*1向量，零偏
% dKScalP: 3*2矩阵，第一列为标度因数误差，第二列为二次项误差
% dPSS0: 3*3矩阵，失准角（安装误差），无单位
% stepNum: 迭代步数，默认为2
%
% Output Arguments:
% in: 3*n矩阵，经补偿后的器件输入值
%
% References:
% [1] 理论文档“惯组误差模型”第3节

if nargin < 6
    stepNum = 2;
end

in = NaN(size(DupsilonTildeOutSl));
dSFMA = diag(dKScalP(:,1)) + dPSS0';
for k=1:size(DupsilonTildeOutSl, 2)
    in(:, k) = DupsilonTildeOutSl(:, k) ./ KScal0 - dfBiasS;
    for i=1:stepNum
        in(:, k) = newtoniter(in(:, k), DupsilonTildeOutSl(:, k), KScal0, dfBiasS, dSFMA, dKScalP(:,2));
    end
end
end

function [in] = newtoniter(in, DupsilonTildeOutSl, KScal0, dfBiasS, dSFMA, SO)
fikDeriv = [ KScal0(1)*(dSFMA(1, 1) + 2*SO(1)*in(1) + 1), KScal0(1)*dSFMA(1, 2), KScal0(1)*dSFMA(1, 3)
    KScal0(2)*dSFMA(2, 1), KScal0(2)*(dSFMA(2, 2) + 2*SO(2)*in(2) + 1), KScal0(2)*dSFMA(2, 3)
    KScal0(3)*dSFMA(3, 1), KScal0(3)*dSFMA(3, 2), KScal0(3)*(dSFMA(3, 3) + 2*SO(3)*in(3) + 1)];
in = in - fikDeriv \ (KScal0.*(in+dfBiasS+dSFMA*in+SO.*(in.^2)) - DupsilonTildeOutSl);
end